#import serial
import numpy as np
import os
import pylab as py
#import usb

class CitMag():
    def __init__(self, port=None, pocet = 1000,  subor=False):
        self.port = port
        self.pocet = pocet
        self.k1 = []
        self.k2 = []
        self.k3 = []
        self.k4 = []
        self.x = []
        self.synab = []
        self.citac = []
        self.mag = []
        self.subor = subor
    
    def Test_Subor(self):
        f = open(self.port, 'r')
        k = f.read()
        i = 1
        mag = []
        while i < len(k):
            mag.append(ord(k[i]))
            i+=1
        f.close()
        return mag
        
    def Citanie(self):
        mag = []
        if self.subor == False:
            i = 1
            s = serial.Serial(self.port)
            s.bytesize = serial.EIGHTBITS
            s.parity = serial.PARITY_NONE
            s.stopbits = serial.STOPBITS_ONE
            s.baudrate = 115200
            k = s.read(self.pocet)
            print len(k)
            s.close()
            while i < len(k):
                mag.append(ord(k[i]))
                i+=1
            #print len(mag)
            return mag
        else:
            f = open(self.port, 'r')
            k = f.read()
            i = 1
            mag = []
            #print k
            #print len(k)
            while i < len(k):
                mag.append(ord(k[i]))
                i+=1
            f.close()
            return mag
    
    def Dekodovanie(self):
        buf = np.zeros(45)
        kk1 = 0.
        kk2 = 0.
        kk3 = 0.
        kk4 = 0.
        n = 0
        self.mag = self.Citanie()
        #print self.mag
        for c in self.mag:
            if self.pocet < n:
                for i in range(1, 31):
                    buf[i] = buf[i+1]
                buf[31] = c
                if (buf[1]==255) and (buf[15+1]==255) and (buf[30+1]==255) and ((buf[14]==1) or (buf[14]==2)) and ((buf[15+14]==1) or (buf[15+14]==2)):
                    k1 = (buf[2] * 2**16 + buf[3] * 2**8 + buf[4])
                    k2 = (buf[5] * 2**16 + buf[6] * 2**8 + buf[7])
                    k3 = (buf[8] * 2**16 + buf[9] * 2**8 + buf[10])
                    k4 = (buf[11] * 2**16 + buf[12] * 2**8 + buf[13])
                    synab = (buf[14])
                    citac = (buf[15])
                    n = n + 1
                    
                    if n > 1:
                        if synab==1:
                            kk1 = k1
                            kk2 = k2
                            kk3 = k3
                            kk4 = k4
                        else:
                            self.k1.append(k1-kk1)
                            self.k2.append(k2-kk2)
                            self.k3.append(k3-kk3)
                            self.k4.append(k4-kk4)
        #print 'k1: '+str(k1)+' k2: '+str(k2)+' k3: '+str(k3)+' k4: '+str(k4)+' synab: '+str(synab)+' citac: '+str(citac)
        #print n
        return self.k1,self.k2,self.k3,self.k4,self.synab,self.citac

#k1, k2, k3, k4, synab, citac = CitMag("/home/gandhi/Dropbox/Magnetometer/1/cat_pato_28-12-2008", 1000, True).Dekodovanie()
#print k2
